/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file gpsParser.cxx
  \brief class implemenation for gpsParser

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <iostream>
#include <math.h>

#include "gpsSimParser.h"
#include "color.h"
#include "GPS.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---External Global Variables---->*/
extern bool verbose;

/*<-Global Variables-->*/

GPSSimParser::GPSSimParser(String name) : ThreadedPipe(name)
{
	has_telem = false;
}

bool GPSSimParser::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);
	last_telem.stamp();
	return true;
}

void GPSSimParser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo

	maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];
	if(fifo_FDs[INBOARD_RX] > maxFD) maxFD = fifo_FDs[INBOARD_RX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("GPSSimParser::update() select");
				execRate = -1;
			} else
				cout << getName() << "GPSSimParser::update() - select interrupted" << endl;
		} else cout << getName() << "::GPSSimParser::update() - select timed out" << endl;
	}
	else {
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs))
		{
			int n=readFifo(INBOARD_RX);
			if( n < 0 && errno != EAGAIN)
				perror("DataToSASSI_Parser::update() - error reading from fifo ");
			else {
				if(n>0) {

					uint16_t length = CMD_SIZE(fifo_msg.data);
					if( length != fifo_msg.size) {
						cout << getName() << "::update() - error, length of command != length of buffer" << endl
							<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

						if( length < 1000 && length <= fifo_msg.size ) {
							if( C_calcCheckSum((uint8_t *)buf) == 0 )
								cout << "\tCMD: is valid" << endl;
							else
								cout << "\tCMD: is NOT valid" << endl;
						}

						return;
					}

					Command cmd((uint8_t*)fifo_msg.data);

					if( !cmd.isValid() )
						cout << "DataToSASSI::update() - error, invalid command in rxfifo"<< endl << cmd ;
					else
						parse(cmd, fifo_msg.src);
				}
			}
		}
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			readFifo(OUTBOARD_RX);
		}
	}

	if(has_telem) {
		currentStamp.stamp();
		if((currentStamp - last_telem) > (TELEMETRY_PERIOD * SEC2MICRO) ) {

			uint8_t * dataPtr = fifo_msg.data;
			char buffer[256];

			//$GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
			sprintf(buffer,"$GPGGA,%02d%02d%02d.000,%d%02.04f,%c,%d%02.04f,%c,1,%02d,%1.1f,%.1f,M,0.0,M,,0000*",
					telemetry.gps.utc.hours,
					telemetry.gps.utc.minutes,
					int(telemetry.gps.utc.seconds),
					int(telemetry.gps.pos.lat),
					(telemetry.gps.pos.lat - (float)(int(telemetry.gps.pos.lat))) * 60.0,
					'N',
					int(fabs(telemetry.gps.pos.lon)),
					(fabs(telemetry.gps.pos.lon) - (float)(int(fabs(telemetry.gps.pos.lon)))) * 60.0,
					'W',
					telemetry.gps.accuracy.sats,
					telemetry.gps.accuracy.pdop,
					telemetry.gps.pos.alt);

			//$GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh
			/*sprintf(buffer,"$GPRMC,%02d%02d%02.03f,%c,%04.04f,%c,%05.04f,%c,%03.02f,%3.02f,%02d%02d%02d,,*",
					telemetry.gps.utc.hours,
					telemetry.gps.utc.minutes,
					telemetry.gps.utc.seconds,
					'A',
					telemetry.gps.pos.lat*100,
					'N',
					fabs(telemetry.gps.pos.lon)*100,
					'W',
					telemetry.gps.sog,
					telemetry.gps.track,
					telemetry.gps.date.day,
					telemetry.gps.date.month,
					telemetry.gps.date.year-2000
					);*/

			const string nmea (buffer);

			uint8_t checksum = GPSCalcCheckSum(nmea);
			int size = sprintf((char *)dataPtr,"%s%x\n\r",nmea.c_str(),checksum);
			//cout << dataPtr << endl;

			fifo_msg.size = size;

			Client * a_client;
			Client * me = shmPtr->clients.getClient(0);

			fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
			fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
			writeFifo(OUTBOARD_TX);

			last_telem.stamp();
		}
	}
}

void GPSSimParser::parse(const Command &cmd, in_addr address) 
{
	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);
	Client * a_client = shmPtr->clients.getClient(address);
	if( !a_client ) {
		cout << "Client not found" << endl;
		return;

	}

	switch(cmd.cmd()) {
		case CMD_TELEM: {
					CopyBufToStruct( dataPtr, telemetry);
					has_telem = true;
					break;
				} 
	}
}
